#include <stdio.h>

#include "eMPL/inv_mpu.h"
#include "eMPL/inv_mpu_dmp_motion_driver.h"
#include "eMPL/empl_driver.h"
#include "driver/i2c.h"

#include "esp_log.h"

#include "i2c_define.h"

char orientation[9];

static const char *TAG = "mainc";
#define DEFAULT_MPU_HZ (200)

static esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

void app_main(void)
{
    i2c_master_init();
    mpu_init();
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(DEFAULT_MPU_HZ);

    // DMP
    dmp_load_motion_driver_firmware();
    int8_t orientation[9] = {1, 0, 0,
                             0, 1, 0,
                             0, 0, 1};
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(orientation));
    // dmp_register_tap_cb(tap_cb);
    // dmp_register_android_orient_cb(android_orient_cb);
    dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                       DMP_FEATURE_GYRO_CAL);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);

    int result;
    long gyro[3], accel[3];
    result = mpu_run_6500_self_test(gyro, accel, 0);
    ESP_LOGI(TAG, "%d, gryo: %ld,accel:%ld", result, gyro[0], accel[0]);

    mpu_set_dmp_state(1);

    float pitch, roll, yaw;
    while (1)
    {
        // uint8_t enabled = 0;
        // mpu_get_dmp_state(&enabled);
        // ESP_LOGI(TAG, "%d", enabled);
        mpu_dmp_get_data(&pitch, &roll, &yaw);
        printf("demo:%f,%f,%f\n", pitch, roll, yaw);

        vTaskDelay(pdMS_TO_TICKS(10));
    }
}
